package com.example.cargetter.utils;
import com.example.cargetter.pojo.CarData;
import org.locationtech.proj4j.*;

/**
 * 处理坐标并判断预警
 */

public class WarnUtil {

    private double ego_lat1;  // 主车经度
    private double ego_lon1;  // 主车纬度


    private double obj_lat2;  // 目标车经度
    private double obj_lon2;  // 目标车纬度
    private double alt;  // 海拔高度

    private double ego_speed;//主车速度
    private double Obj_speed;//目标车速度

    private int flag;    //判断后的警示标识。0代表安全，1代表前车碰撞预警，2代表严重预警。
    public WarnUtil(){}


    public WarnUtil(double ego_lat1, double ego_lon1, double obj_lat2, double obj_lon2, double alt, double ego_speed, double Obj_speed) {
        this.ego_lat1 = ego_lat1;
        this.ego_lon1 = ego_lon1;
        this.obj_lat2 = obj_lat2;
        this.obj_lon2 = obj_lon2;
        this.alt = alt;
        this.ego_speed = ego_speed;
        this.Obj_speed = Obj_speed;
    }


    public void judge() {

        //主车xy
        double x1 = getX(this.ego_lon1, this.ego_lat1, this.alt);
        double y1 = getY(this.ego_lon1, this.ego_lat1, this.alt);

        //目标车xy
        double x2 = getX(this.obj_lon2, this.obj_lat2, this.alt);
        double y2 = getY(this.obj_lon2, this.obj_lat2, this.alt);

        //主车偏航角
        double ego_yaw = Math.atan2(y1, x1);

        //目标车偏航角
        double obj_yaw = Math.atan2(y2, x2);


        //前方碰撞预警
        double longitudinal_offset = Math.abs(x1 - x2);
        double lateral_offset = Math.abs(y1 - y2);
//        int flagx = 0;

        //计算两车间的距离
        double distance = Math.sqrt(Math.pow(longitudinal_offset, 2) + Math.pow(lateral_offset, 2));
        CarData carData = CarData.getInstance(0, 0, 0, 0);
        carData.setDistance(distance);

        obj_yaw = (360 - obj_yaw + 90);
        if (obj_yaw >= 360) {
            obj_yaw -= 360;
        }

        //y_axle_speed_offset 表示两辆车在 Y 轴方向上的速度偏差
        //x_axle_speed_offset 表示两辆车在 X 轴方向上的速度偏差
        //V_error 表示两辆车在车轴方向上的速度偏差的模
        double y_axle_speed_offset = this.ego_speed * Math.sin(ego_yaw) - this.Obj_speed * Math.sin(Math.toRadians(obj_yaw));
        double x_axle_speed_offset = this.ego_speed * Math.cos(ego_yaw) - this.Obj_speed * Math.cos(Math.toRadians(obj_yaw));
        double V_error = Math.sqrt(Math.pow(y_axle_speed_offset, 2) + Math.pow(x_axle_speed_offset, 2));
        //TTC是预计碰撞时间
        double TTC = distance / V_error;
        System.out.println("ttc: " + TTC + ", V_error: " + V_error + "222 " + Math.abs(y1 - y2) + ", distance: " + distance + ", Math.abs(ego_yaw - obj_yaw): " + Math.abs(ego_yaw - obj_yaw));

        //碰撞危险,1是前方碰撞，2是交叉路口
        if (distance < 20 && Math.abs(Math.toDegrees(ego_yaw) - obj_yaw) < 10) {
            //不在同一车道，说明是交叉路口碰撞
            if (Math.abs(ego_yaw - obj_yaw) < 10 && Math.abs(y1 - y2) < 3) {
                flag = 2;
            } else flag =0;

            } else {
                //是前方碰撞
            if(TTC < 5) flag = 1;
            else flag =0;
//                //3<TTC<5是轻微预警，在仿真中是轻微踩刹车减速的
//                if (TTC > 3 && TTC < 5) flag = 1;
//                    // TTC<3是严重预警，在仿真中是踩急刹车制动的
//                else if (TTC < 3) flag = 1;
//                else {
//                    flag = 0;
//            }
//        System.out.println("ttc: " + TTC + ", V_error: " + V_error);
//        System.out.println("flag= "+ flag);
        }
    }


    //获得x坐标
    public static double getX(double lon, double lat, double alt) {
        // 创建投影对象
        CRSFactory crsFactory = new CRSFactory();
        CoordinateReferenceSystem crsLatLon = crsFactory.createFromName("EPSG:4326");
        CoordinateReferenceSystem crsCartesian = crsFactory.createFromName("EPSG:3857");
        BasicCoordinateTransform transform = new BasicCoordinateTransform(crsLatLon, crsCartesian);

        // 经纬度转换为空间直角坐标系坐标
        ProjCoordinate source = new ProjCoordinate(lon, lat, alt);
        ProjCoordinate target = new ProjCoordinate();
        transform.transform(source, target);

        // 输出结果
        double x = target.x;  // x坐标
        return x;
    }

    //获得Y坐标
    public static double getY(double lon, double lat, double alt) {
        // 创建投影对象
        CRSFactory crsFactory = new CRSFactory();
        CoordinateReferenceSystem crsLatLon = crsFactory.createFromName("EPSG:4326");
        CoordinateReferenceSystem crsCartesian = crsFactory.createFromName("EPSG:3857");
        BasicCoordinateTransform transform = new BasicCoordinateTransform(crsLatLon, crsCartesian);

        // 经纬度转换为空间直角坐标系坐标
        ProjCoordinate source = new ProjCoordinate(lon, lat, alt);
        ProjCoordinate target = new ProjCoordinate();
        transform.transform(source, target);

        // 输出结果
        double y = target.y;  // x坐标
        return y;
    }


    public int getFlag() {
        return flag;
    }

    public void setFlag(int flag) {
        this.flag = flag;
    }
}
